function [nofusion] = fcn_nofusion(gyro, accb, magb, stoch_p, q_initial, pos_initial, no_int, acc1g, dt)
%fcn_nofusion 	This function calculates attitude and position data from
%               inertial measurements.

%   [nofusion] = fcn_nofusion(gyro, accb, magb, stoch_p, q_initial, vel_initial, pos_initial, no_int, acc1g, dt)
%   This function calculates attitude and position data from inertial 
%   measurements.

%   IN      gyro        Gyro measurements error-prone
%   IN      accb        Acc measurements error-prone
%   IN      magb        Mag measurements error-prone
%   IN      stoch_p     Vector containing stochastic parameters for error modells
%   IN      q_initial   Initial quaternion attitude
%   IN      vel_initial Initial velocity
%   IN      pos_initial Initial position
%   IN      no_int      Number of samples before position integration starts (time for the algorithm to reach a steady state)
%   IN      acc1g       Gravity (equals the gravity at Analog Devices Calibration site)
%   IN      dt          Sample period
%   OUT     nofusion    Output data


%% Get QUEST initial attitude if wanted
if strcmpi(q_initial, 'QUEST')
	quest_noise = [stoch_p.Nacc; stoch_p.Nmag];
    obs = [ [accb(1,:)/norm(accb(1,:))]', [magb(1,:)/norm(magb(1,:))]'];
    ref = [[0, 0, 1]', [1, 0, 0]'];
    q_initial = quest(obs, ref, quest_noise, 'noise vectors', 1);    
end

%% For algorithm
% Get loop length
k = length(gyro);

% Save for output
pos_save = zeros(k, 3);                         % position
vel_save = zeros(k, 3);                         % velocity
q_save   = zeros(k, 4);                         % quaternion attitude

% Initials
q = q_initial;
v = zeros(1, 3);
p = pos_initial;

% Other stuff...
flag = 1;

for j=1:k,j;

	% Get current data for the sake of readability...
    wm = gyro(j,:);
	am = accb(j,:);
    mm = magb(j,:);
    
	% Propagate quaternion
    q = dq_multi_trig(q, wm, dt, 'norm');  
        
    % Save for output
    q_save(j,:) = q;
        
    %% ONLY start position integration if ss_time is over (algorithm and
    % attitude reached steady state)
    if (j > no_int)
        
        % RESET attitude
        % The initial attitude at the beginning of this function is equal
        % to the attitude where the fusion algorithm starts position
        % integration. So in order to compare algorithms the initial
        % attitude when first integrating to position has to be equal in
        % all functions. Since noisy data offsets the attitude before
        % starting position integration we will reset it here. Comment the
        % following lines if this behavior is not wanted !!!
        if (flag == 1)
            flag = 0;
            q = q_initial;
            
            q_save(j,:) = q;
        end
        
        % Velocity and Position
        % Transfer acc vector to NED (inertial) frame
        acc_earth = quatmultiply(q, quatmultiply([0 am], quatconj(q)));
        acc_earth = acc_earth(1, 2:4);      

        % Substract 1g from z-component
        acc_earth(3) = acc_earth(3) - acc1g;

        % Integrate to velocity
        v = v + acc_earth * dt;
        
        % Save for output
        vel_save(j,:) = v;

        % Integrate to position
        p = p + v * dt;
        
        % Save for output
        pos_save(j,:) = p;
    
    else
        % Save for output        
        vel_save(j,:) = v;
        pos_save(j,:) = p;
   
    end 
    
end


%% Output
% Eulerangles from quaternion attitude
[yaw, pitch, roll] = quat2angle( q_save, 'ZYX' );

% Transform vectors to inertial frame
tmp(:,2:4) = accb;
acci_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));
tmp(:,2:4) = magb;
magi_save = quatmultiply(q_save, quatmultiply(tmp, quatconj(q_save)));

% Output sample time
for j=1:k,j;
    nofusion.time(j,1) = j*dt;
end

% Output position
nofusion.X = pos_save(:,1);
nofusion.Y = pos_save(:,2);
nofusion.Z = pos_save(:,3);
nofusion.pos = pos_save;

% Output velocity
nofusion.vel = vel_save;
nofusion.v_xy = (vel_save(:,1).^2 + vel_save(:,2).^2).^(1/2);

% Output acceleration
nofusion.a_xy = (acci_save(:,2).^2 + acci_save(:,3).^2).^(1/2);

% Output euler angles
nofusion.phi = roll;
nofusion.theta = pitch;
nofusion.psi = yaw;

% Output quaternion attitude
nofusion.q = q_save;

% Output quaternion attitude where position integration starts
nofusion.q_int = q_save(no_int+1,:);

% Output direction vectors: acceleration
nofusion.acci = acci_save(:,2:4);

% Output direction vectors: magnetometer
nofusion.magi = magi_save(:,2:4);

end